/*************************************************
*      Filename: distance_twomesh.cu
*        Coding: utf-8
*        Author: Futuan Li
*         Email: lifutuan@galasports.net
*      HomePage: http://www.galasports.net
*  Copyright(C): InnoReal Technology Co.,Ltd.
*       Version: 0.0.1
*    CreateDate: 2018-04-24 15:46:41
*    LastChange: 2018-06-22 17:39:50
*          Desc: 
*       History: 
*************************************************/

#include "distance_twomesh.h"
#include <innoreal/utils/innoreal_helper.hpp>
#include "constant.h"
#define  HAUSDORFF_DIS_INF 1000000
DisTwoMesh::DisTwoMesh() {
  threadpool_i = new innoreal::InnoRealThreadpool(2);
  threadpool_j = new innoreal::InnoRealThreadpool(1);

}

DisTwoMesh::~DisTwoMesh() {
  delete threadpool_i;
  delete threadpool_j;
}

double DisTwoMesh::Hausdorff_KD(InputData *data) {
  thrust::device_vector<float4> d_source_data = data->source_.d_vertex_;
  int num_source_data = data->source_.num_vertex_;
  int k = 1;
  float distance = 0;
  thrust::device_vector<float> d_knn_distance_;
  d_knn_distance_.resize(num_source_data);
  thrust::device_vector<int> d_knn_index;
  d_knn_index.resize(num_source_data);

  data->flann_search_knn_.GetKnnResult(RAW_PTR(data->source_.d_vertex_),
                                       num_source_data,
                                       k,
                                       RAW_PTR(d_knn_index),
                                       RAW_PTR(d_knn_distance_));

  for (int i = 0; i < num_source_data; i++) {
    if (d_knn_distance_[i] > distance) {
      distance = d_knn_distance_[i];
    }
  }
  distance = sqrt(distance);
  return distance;
}

double DisTwoMesh::Hausdorff_Eigen(InputData *data) {
  double distance = 0.0, temp = 0;
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  int va_N = data->source_.num_vertex_;
  int vb_N = data->target_.num_vertex_;
  MatrixXd A(vb_N, 3), B(vb_N, 3), C(vb_N, 3);
  MatrixXd a(1, 3), l(vb_N, 1);
  for (int i = 0; i < vb_N; i++) {
    B(i, 0) = v_b[i].x;
    B(i, 1) = v_b[i].y;
    B(i, 2) = v_b[i].z;
  }
  for (int i = 0; i < va_N; i++) {
    a(0, 0) = v_a[i].x;
    a(0, 1) = v_a[i].y;
    a(0, 2) = v_a[i].z;
    A = a.replicate(vb_N, 1);
    C = A - B;
    C = C.array() * C.array();
    l = C.rowwise().sum();
    temp = l.minCoeff();
    distance = temp > distance ? temp : distance;
  }

  distance = sqrt(distance);
  return distance;
}

double DisTwoMesh::Hausdorff_Simple(InputData *data) {
  double distance = 0.0;
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  int va_N = data->source_.num_vertex_;
  int vb_N = data->target_.num_vertex_;
  double dis_temp;
  double dis_temp1 = 0, dis_temp2;

  for (int i = 0; i < va_N; i++) {
    dis_temp2 = 1000000;
    for (int j = 0; j < vb_N; j++) {
      dis_temp = (v_a[i].x - v_b[j].x) * (v_a[i].x - v_b[j].x) + (v_a[i].y - v_b[j].y) * (v_a[i].y - v_b[j].y)
          + (v_a[i].z - v_b[j].z) * (v_a[i].z - v_b[j].z);
      if (dis_temp < dis_temp2) {
        dis_temp2 = dis_temp;
      }
    }
    if (dis_temp2 > dis_temp1) {
      dis_temp1 = dis_temp2;
    }
  }
  distance = sqrt(dis_temp1);
  return distance;
}

// modified by lz paralle
double DisTwoMesh::Hausdorff_Simple_par2(InputData *data) {
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  int va_N = data->source_.num_vertex_;
  int vb_N = data->target_.num_vertex_;
  int threadnumi = threadpool_i->size();
  int threadnumj = threadpool_j->size();
//  double J[threadnumi][vb_N];
  vector<vector<double>> J(threadnumi);
  for (int i = 0; i < J.size(); i++)
    J[i].resize(vb_N);

  std::vector<double> vertexs_dis_J(vb_N, HAUSDORFF_DIS_INF);
  std::vector<double> vertexs_dis_mini(va_N, HAUSDORFF_DIS_INF);

  double global_max_dis = 0;


  auto compute_vertex_disj=[&](int tidi, int tid, float4 vertex, size_t job_beg, size_t job_end, size_t job_step){
    for(int vertexidxj = job_beg; vertexidxj<job_end; vertexidxj += job_step){
//      vertexs_dis_J[vertexidxj]=(vertex.x - v_b[vertexidxj].x) * (vertex.x - v_b[vertexidxj].x)
//          + (vertex.y - v_b[vertexidxj].y) * (vertex.y - v_b[vertexidxj].y)
//          + (vertex.z - v_b[vertexidxj].z) * (vertex.z - v_b[vertexidxj].z);
//      INNOREALI("threadi:%d--------threadj:%d\n",tidi,tid);
      J[tidi][vertexidxj]=(vertex.x - v_b[vertexidxj].x) * (vertex.x - v_b[vertexidxj].x)
          + (vertex.y - v_b[vertexidxj].y) * (vertex.y - v_b[vertexidxj].y)
          + (vertex.z - v_b[vertexidxj].z) * (vertex.z - v_b[vertexidxj].z);
//      INNOREALI("threadi%d:%d,%d------------%f\n", tidi, tidi, vertexidxj, J[tidi][vertexidxj]);

    }
  };

  size_t jobsizej = (vb_N + threadnumj - 1) / threadnumj;
  size_t joboffsetj = 0;
  size_t jobleftj = vb_N;
  auto compute_vertex_disi = [&](int tid, size_t job_beg, size_t job_end, size_t job_step) {
    INNOREALI("threadi:%d--------\n",tid);
    for (int vertexidxi = job_beg; vertexidxi < job_end; vertexidxi += job_step) {
      float4 vertexi = v_a[vertexidxi];
      size_t joboffsetj = 0;
      size_t jobleftj = vb_N;
      for (int tidj = 0; tidj < threadnumj; tidj++) {
//        INNOREALI("threadi:%d--------threadj:%d:\n",tid,tidj);
        size_t threadjobsizej = std::min(jobsizej, jobleftj);
        threadpool_j->pushjob(compute_vertex_disj, tid, tidj, vertexi, joboffsetj, joboffsetj + threadjobsizej, 1);
        joboffsetj += threadjobsizej;
        jobleftj -= threadjobsizej;
        if (jobleftj <= 0) break;
      }
      threadpool_j->sync();
//      INNOREALI("tidi: %d, min_element:%d--------%d, minValue:%f\n",tid, 0, J[tid].size(), *min_element(J[tid].begin(),J[tid].end()));
//      vertexs_dis_mini[vertexidxi] = *min_element(vertexs_dis_J.begin(),vertexs_dis_J.end());
      vertexs_dis_mini[vertexidxi] = *min_element(J[tid].begin(),J[tid].end());

    }
  };

  size_t jobsizei = (va_N + threadnumi - 1) / threadnumi;
  size_t joboffseti = 0;
  size_t joblefti = va_N;
  for (int tidi = 0; tidi < threadnumi; tidi++) {
    INNOREALI("thread number :------------------ %d\n",tidi);

    size_t threadjobsizei = std::min(jobsizei, joblefti);
    threadpool_i->pushjob(compute_vertex_disi, tidi, joboffseti, joboffseti + threadjobsizei, 1);
    joboffseti += threadjobsizei;
    joblefti -= threadjobsizei;
    if (joblefti <= 0) break;
  }
  threadpool_i->sync();

  global_max_dis=*max_element(vertexs_dis_mini.begin(),vertexs_dis_mini.end());

  return sqrt(global_max_dis);
}

double DisTwoMesh::Hausdorff_Simple_par1(InputData *data) {
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  int va_N = data->source_.num_vertex_;
  int vb_N = data->target_.num_vertex_;
  int threadnum = threadpool_i->size();
  std::vector<double> vertexs_dis_minj(vb_N, HAUSDORFF_DIS_INF);
  std::vector<double> vertexs_dis_min(va_N, HAUSDORFF_DIS_INF);
  std::vector<double> hausdorff_parallel(threadnum, 0);
  double global_max_dis = 0;


  auto compute_vertex_dis = [&](int tid, size_t job_beg, size_t job_end, size_t job_step) {
    for (int vertexidx = job_beg; vertexidx < job_end; vertexidx += job_step) {
      double dis_temp;
      double min_dis_temp = HAUSDORFF_DIS_INF;
      float4 vertex = v_a[vertexidx];
      for (int j = 0; j < vb_N; j++) {
        dis_temp = (vertex.x - v_b[j].x) * (vertex.x - v_b[j].x)
            + (vertex.y - v_b[j].y) * (vertex.y - v_b[j].y)
            + (vertex.z - v_b[j].z) * (vertex.z - v_b[j].z);
        if (dis_temp < min_dis_temp) {
          min_dis_temp = dis_temp;
        }
      }
      vertexs_dis_min[vertexidx] = min_dis_temp;
    }
  };

  size_t jobsize = (va_N + threadnum - 1) / threadnum;
  size_t joboffset = 0;
  size_t jobleft = va_N;
  for (int tid = 0; tid < threadnum; tid++) {
        size_t threadjobsize = std::min(jobsize, jobleft);
        threadpool_i->pushjob(compute_vertex_dis, tid, joboffset, joboffset + threadjobsize, 1);
        joboffset += threadjobsize;
        jobleft -= threadjobsize;
        if (jobleft <= 0) break;
  }
  threadpool_i->sync();

  global_max_dis=*max_element(vertexs_dis_min.begin(), vertexs_dis_min.end());

  return sqrt(global_max_dis);
}

// modified by lz
double DisTwoMesh::Hausdorff_Simple_par(InputData *data) {
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  int va_N = data->source_.num_vertex_;
  int vb_N = data->target_.num_vertex_;
  int threadnum = threadpool_i->size();
  std::vector<double> vertexs_dis_min(va_N, HAUSDORFF_DIS_INF);
  std::vector<double> hausdorff_parallel(threadnum, 0);
  double global_max_dis = 0;


  auto compute_vertex_dis = [&](int tid, size_t job_beg, size_t job_end, size_t job_step) {
    for (int vertexidx = job_beg; vertexidx < job_end; vertexidx += job_step) {
      double dis_temp;
      double min_dis_temp = HAUSDORFF_DIS_INF;
      float4 vertex = v_a[vertexidx];
      for (int j = 0; j < vb_N; j++) {
        dis_temp = (vertex.x - v_b[j].x) * (vertex.x - v_b[j].x)
            + (vertex.y - v_b[j].y) * (vertex.y - v_b[j].y)
            + (vertex.z - v_b[j].z) * (vertex.z - v_b[j].z);
        if (dis_temp < min_dis_temp) {
          min_dis_temp = dis_temp;
        }
      }
      vertexs_dis_min[vertexidx] = min_dis_temp;
    }
  };

  size_t jobsize = (va_N + threadnum - 1) / threadnum;
  size_t joboffset = 0;
  size_t jobleft = va_N;
  for (int tid = 0; tid < threadnum; tid++) {
#if 0
    threadpool_->pushjob(compute_vertex_dis, tid, tid, va_N, threadnum);
#else
    size_t threadjobsize = std::min(jobsize, jobleft);
    threadpool_i->pushjob(compute_vertex_dis, tid, joboffset, joboffset + threadjobsize, 1);
    joboffset += threadjobsize;
    jobleft -= threadjobsize;
    if (jobleft <= 0) break;
#endif
  }
  threadpool_i->sync();


  auto find_max_dis = [&](int tid, size_t job_beg, size_t job_end, size_t job_step) {
    double dis_max = 0;
    for (size_t i = job_beg; i < job_end; i += job_step) {
      if (vertexs_dis_min[i] > dis_max) dis_max = vertexs_dis_min[i];
    }
    hausdorff_parallel[tid] = dis_max;
  };

  jobsize = (va_N + threadnum - 1) / threadnum;
  joboffset = 0;
  jobleft = va_N;
  for (int tid = 0; tid < threadnum; tid++) {
    size_t threadjobsize = std::min(jobsize, jobleft);
    threadpool_i->pushjob(find_max_dis, tid, joboffset, joboffset + threadjobsize, 1);
    joboffset += threadjobsize;
    jobleft -= threadjobsize;
    if (jobleft <= 0) break;
  }
  threadpool_i->sync();
  for (int tid = 0; tid < threadnum; tid++) {
    if (hausdorff_parallel[tid] > global_max_dis) global_max_dis = hausdorff_parallel[tid];
  }

  return sqrt(global_max_dis);
}

double DisTwoMesh::Hausdorff_igl(InputData *data) {
  //distance = 4;
  //printf("test %f  ms\n", distance);
  double distance = 10;
  thrust::host_vector<float4> v_a = data->source_.d_vertex_;
  thrust::host_vector<int4> f_a = data->source_.d_face_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  thrust::host_vector<int4> f_b = data->target_.d_face_;
  int va_N = data->source_.num_vertex_;
  int fa_N = data->source_.num_face_;
  int vb_N = data->target_.num_vertex_;
  int fb_N = data->target_.num_face_;
  Eigen::MatrixXf Va(va_N, 3), Vb(vb_N, 3);//mesh1
  Eigen::MatrixXi Fa(fa_N, 3), Fb(fb_N, 3);//
  for (int ite = 0; ite < va_N; ite++) {
    Va(ite, 0) = v_a[ite].x;
    Va(ite, 1) = v_a[ite].y;
    Va(ite, 2) = v_a[ite].z;
  }
  for (int ite = 0; ite < fa_N; ite++) {
    Fa(ite, 0) = f_a[ite].x;
    Fa(ite, 1) = f_a[ite].y;
    Fa(ite, 2) = f_a[ite].z;
  }
  for (int ite = 0; ite < vb_N; ite++) {
    Vb(ite, 0) = v_b[ite].x;
    Vb(ite, 1) = v_b[ite].y;
    Vb(ite, 2) = v_b[ite].z;
  }
  for (int ite = 0; ite < fa_N; ite++) {
    Fb(ite, 0) = f_b[ite].x;
    Fb(ite, 1) = f_b[ite].y;
    Fb(ite, 2) = f_b[ite].z;
  }
  igl::hausdorff(Va, Fa, Vb, Fb, distance);
  return distance;
}

double DisTwoMesh::Hausdorff_Average(InputData *data) {
  //distance = 4;
  //printf("test %f  ms\n", distance);
  double distance = 0.0;
  thrust::host_vector<float4> v_a = data->deformed_.d_vertex_;
  thrust::host_vector<int4> f_a = data->deformed_.d_face_;
  thrust::host_vector<float4> v_b = data->target_.d_vertex_;
  thrust::host_vector<int4> f_b = data->target_.d_face_;
  int va_N = data->deformed_.num_vertex_;
  int fa_N = data->deformed_.num_face_;
  int vb_N = data->target_.num_vertex_;
  int fb_N = data->target_.num_face_;
  double dis_temp;
  double sum = 0.0;
  double dis_temp2 = 1000000;
  int va_N_k = va_N;
  for (int i = 0; i < va_N; i++) {
    dis_temp2 = 1000000;//距离不能过大，不然认为奇异点
    for (int j = 0; j < vb_N; j++) {
      if (i != j) {
        dis_temp = sqrt((v_a[i].x - v_b[j].x) * (v_a[i].x - v_b[j].x) + (v_a[i].y - v_b[j].y) * (v_a[i].y - v_b[j].y) +
            (v_a[i].z - v_b[j].z) * (v_a[i].z - v_b[j].z));
        if (dis_temp2 > 1000) {
          dis_temp2 = dis_temp;
        } else {
          dis_temp2 = (dis_temp < dis_temp2) ? dis_temp : dis_temp2;
        }
        //if(i==45){
        //	std::cout << "dis_temp2: " << dis_temp2 << std::endl;
        //}
      }
    }

    if (dis_temp2 < 1000) {
      sum += dis_temp2;
    } else
      va_N_k -= 1;
  }
  if (va_N_k > 0)
    distance = sum / va_N_k;
  else {
    INNOREALI("Please check your datas again! \n");
    printf("Output the va_N_k:%d\n", va_N_k);
  }
  return distance;
}
